#!/usr/bin/python3
# The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
#
#   This example shows how to use dlib's face recognition tool.  This tool maps
#   an image of a human face to a 128 dimensional vector space where images of
#   the same person are near to each other and images from different people are
#   far apart.  Therefore, you can perform face recognition by mapping faces to
#   the 128D space and then checking if their Euclidean distance is small
#   enough. 
#
#   When using a distance threshold of 0.6, the dlib model obtains an accuracy
#   of 99.38% on the standard LFW face recognition benchmark, which is
#   comparable to other state-of-the-art methods for face recognition as of
#   February 2017. This accuracy means that, when presented with a pair of face
#   images, the tool will correctly identify if the pair belongs to the same
#   person or is from different people 99.38% of the time.
#
#   Finally, for an in-depth discussion of how dlib's tool works you should
#   refer to the C++ example program dnn_face_recognition_ex.cpp and the
#   attendant documentation referenced therein.
#
#
#
#
# COMPILING/INSTALLING THE DLIB PYTHON INTERFACE
#   You can install dlib using the command:
#       pip install dlib
#
#   Alternatively, if you want to compile dlib yourself then go into the dlib
#   root folder and run:
#       python setup.py install
#
#   Compiling dlib should work on any operating system so long as you have
#   CMake installed.  On Ubuntu, this can be done easily by running the
#   command:
#       sudo apt-get install cmake
#
#   Also note that this example requires Numpy which can be installed
#   via the command:
#       pip install numpy

import sys
import os
import time
import datetime
import json
import cv2
import numpy as np
import camreader

list_pointspair = []
imgH_vr = 480 #380
imgW_vr = 640 #672
imgH_ir = 480 #240
imgW_ir = 640 #320
imgneednum = 4
cv2.namedWindow("showWindow", cv2.WINDOW_NORMAL)

def cutFrames():
    imgindex = 0

    # read cam
    # include visible radiation frame and infrared radiation frame
    # using onvif
    while True:
        print("init cam ...")
        #src_camera_vr = "rtsp://admin:qd123456@10.39.245.253:554/h265/ch1/main/av_stream"
        #src_camera_ir = "rtsp://admin:qd123456@10.39.245.253:554/h265/ch2/main/av_stream"
        #src_camera_vr = "rtsp://admin:qd123123@10.39.245.249/h265/ch1/main/av_stream"
        #src_camera_ir = "rtsp://admin:qd123123@10.39.245.249/h265/ch2/main/av_stream"
        src_camera_vr = "rtsp://admin:qd123456@10.39.67.77/h265/ch1/main/av_stream"
        src_camera_ir = "rtsp://admin:qd123456@10.39.67.77/h265/ch2/main/av_stream"
        rtscap_vr = camreader.RTSCapture.create(src_camera_vr) # visible radiation
        rtscap_ir = camreader.RTSCapture.create(src_camera_ir) # infrared radiation
        rtscap_vr.start_read()
        rtscap_ir.start_read()
        print("cam init done.")

        errcode = 0
        errtimes = 0
        sleeptime = 0.05
        while rtscap_vr.isStarted() and rtscap_ir.isStarted():
            print("please enter space key to save current frame.")
            if imgindex >= imgneednum:
                break

            if errtimes > 5:
                print("reconnect to IPC.")
                break

            time.sleep(sleeptime)

            #read frame
            try:
                ok1, frame_vr = rtscap_vr.read_latest_frame()
                ok1, frame_ir = rtscap_ir.read_latest_frame()
                frame_vr = cv2.resize(frame_vr, (imgW_vr, imgH_vr))
            except:
                print("get frame err.")
                errtimes += 1
                continue

            #show 
            try:
                showimg = np.hstack([frame_vr, frame_ir])
                cv2.imshow("showWindow", showimg)
                if cv2.waitKey(1) & 0xFF == ord(' '):
                    cv2.imwrite("frame"+str(imgindex)+".jpg", showimg)
                    imgindex += 1
                    print("img index ", imgindex, " saved.")
            except:
                print("err")

        if imgindex >= imgneednum:
            break

        rtscap_vr.stop_read()
        rtscap_ir.stop_read()
        rtscap_vr.release()
        rtscap_ir.release()
    cv2.destroyAllWindows()


def savePointsPair(event, x, y, flags, param):
    if event == cv2.EVENT_LBUTTONDOWN:
        print("point pos: ", x, y)
        list_pointspair.append((x,y))    


def clickPointsByUser():
    cv2.namedWindow("click points")
    cv2.setMouseCallback("click points", savePointsPair)
    
    for index in range(imgneednum):
        curimgpath = "frame" + str(index) + ".jpg" 
        img = cv2.imread(curimgpath)
        cv2.imshow("click points", img)
        if cv2.waitKey(0) & 0xFF == ord(' '):
            print("next photo")
            continue
    print("finish.")


def calH():
    pt_vr = []
    pt_ir = []
    for i in range(4):
        pt_vr.append(list_pointspair[2 * i])
        pt_ir.append(list_pointspair[2 * i + 1])

    np_vr = np.float32([[pt_vr[0][0],pt_vr[0][1]], \
                        [pt_vr[1][0],pt_vr[1][1]], \
                        [pt_vr[2][0],pt_vr[2][1]], \
                        [pt_vr[3][0],pt_vr[3][1]]])

    np_ir = np.float32([[pt_ir[0][0]-imgW_vr,pt_ir[0][1]], \
                        [pt_ir[1][0]-imgW_vr,pt_ir[1][1]], \
                        [pt_ir[2][0]-imgW_vr,pt_ir[2][1]], \
                        [pt_ir[3][0]-imgW_vr,pt_ir[3][1]]])
    print("np_vr:", np_vr)
    print("np_ir:", np_ir)
    
    H = cv2.getPerspectiveTransform(np_vr, np_ir)
    H = H.tolist()

    return H


if  __name__ == "__main__":
    # press space button to save frame. 4 frames needed
    cutFrames()

    # click mouse left button to mark key point. 
    # click 1 point on visible frame, than click 1 point on IR frame
    # than press space button to change to next frame
    # than repeat
    # 4 point-pairs are needed
    clickPointsByUser()
    print("list_pointspair: ", list_pointspair)

    # calulate H
    H = calH()
    print("H: ", H)

    # save H
    dict = {}
    dict.update({"posCalibM":H})
    result = json.dumps(dict)
    with open("posCalibData.dat", "w", encoding="utf-8") as f: 
        f.write(result)






